/*
 * frsky.cpp
 *
 * Created: 01.05.2012 15:24:07
 *  Author: Maxim Karban <maxim.karban@gmail.com>
 */ 

#include <avr/io.h>
#include <avr/interrupt.h>

#include <stdint.h>
#include "uart.h"
#include "frsky.h"


void frSkyInit(void){
  // setup UART: (TX) as output
  //              BAUDRATE = 9600 for frsky
  uartInit(true, false, 9600);

}

uint8_t uartTXBufferHead = 0;
uint8_t uartTXBufferTail = 0;
uint8_t uartTXBuffer[FRSKY_SEND_BUFFER_SIZE];

bool uartStopped = true;

ISR(USART_TX_vect){
  if(uartTXBufferTail != uartTXBufferHead){
    UDR0 = uartTXBuffer[uartTXBufferTail];
    (uartTXBufferTail)++;
    if((uartTXBufferTail) >= (FRSKY_SEND_BUFFER_SIZE))
      (uartTXBufferTail) = 0;
    uartStopped = false;
  }else{
    uartStopped = true;
  }
}


void putFrskyByte(uint8_t _char){
  uartTXBuffer[uartTXBufferHead++] = _char;
  if(uartTXBufferHead >= FRSKY_SEND_BUFFER_SIZE)
    uartTXBufferHead = 0;
}


void printFrSkyValueInt(int16_t val, uint8_t id){
  putFrskyByte(0x5E);//header
  putFrskyByte(id);
  putFrskyByte(val);
  putFrskyByte(val >> 8);
  printFrSkyRoundBuf();
}

void printFrSkyValueUInt(uint16_t val, uint8_t id){
  putFrskyByte(0x5E);//header
  putFrskyByte(id);
  putFrskyByte(val);
  putFrskyByte(val >> 8);
  printFrSkyRoundBuf();
}

void printFrSkyRoundBuf(){
  if(uartStopped){
    if(uartTXBufferTail != uartTXBufferHead){
      if(UCSR0A & (1 << UDRE0)){
        UDR0 = uartTXBuffer[uartTXBufferTail];
        (uartTXBufferTail)++;
        if((uartTXBufferTail) >= (FRSKY_SEND_BUFFER_SIZE))
          (uartTXBufferTail) = 0;
      }
    }
  }  
}

